package Controllers;

import Behaviors.Arbitrators.Arbitrator;
import Robot.*;
import Robot.Robot;
import Behaviors.Schemas.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;
import Utils.*;


public class NavigationController extends Controller implements Runnable{
    private Arbitrator arbitrator;
    private Robot robot;
    private String name;
    private int sleepTime;
    private boolean isThreaded = true;
    private Vector2d pointArray[];


    private int nextPoint = 1;

    public NavigationController(String name, Robot robot, Arbitrator arbitrator,int sleepTimeMs) {
        super(name,robot);
        this.arbitrator=arbitrator;
        this.robot=robot;
        this.name=name;
        this.sleepTime =sleepTimeMs;
//
        Vector2d point1 = new Vector2d(15.0f,25.0f,Vector2d.MODE_XY);
        Vector2d point2 = new Vector2d(15.0f,19.5f,Vector2d.MODE_XY);
        Vector2d point3 = new Vector2d(20.7f,19.5f,Vector2d.MODE_XY);
        Vector2d point4 = new Vector2d(20.7f,25.0f,Vector2d.MODE_XY);

//        Vector2d point1 = new Vector2d(1.23f,0.0f,Vector2d.MODE_XY);
//        Vector2d point2 = new Vector2d(1.23f,1.23f,Vector2d.MODE_XY);
//        Vector2d point3 = new Vector2d(0.0f,1.23f,Vector2d.MODE_XY);
//        Vector2d point4 = new Vector2d(0.0f,0.0f,Vector2d.MODE_XY);

        pointArray = new Vector2d[4];
        this.pointArray[0] = point1;
        this.pointArray[1] = point2;
        this.pointArray[2] = point3;
        this.pointArray[3] = point4;


    }

    //defines wheter the controller will run as a thread or not
    public void runThreadedMode(boolean isThreaded)
    {
        this.isThreaded = isThreaded;
    }

    public void run(){

        if(isThreaded)
        {
            while(true)
            {
                 //robot.getConnection().requestData();
                 arbitrator.step();
                 Schema schemas[] = arbitrator.getSchemas();

                 MSGoToPoint gotopoint = (MSGoToPoint) schemas[0];

                 Vector2d pos = gotopoint.myPosition.getPosition();
                 Vector2d gp = gotopoint.goalPoint;

                 if(Math.abs(gp.sub(pos).getMag())<0.1f)
                 {
                    gotopoint.changeGoalPoint(pointArray[nextPoint++]);
                 }

                 if(nextPoint > 3)
                     nextPoint = 0;



                 if(isDebugging)
                     System.out.println(name);

                try{
                    //Thread.yield();
                    Thread.sleep(sleepTime);

                }

                catch(Exception e){
                    e.printStackTrace();
                }
            }
        }

        else
        {
            arbitrator.step();
            if(isDebugging)
                System.out.println(name);
        }

    }

    public Arbitrator getArbitrator() {
        return arbitrator;
    }

    public void setArbitrator(Arbitrator val) {
        this.arbitrator = val;
    }
}
